#!/usr/bin/env pybricks-micropython

from pybricks.hubs import EV3Brick
from pybricks.ev3devices import (
    Motor,
    TouchSensor,
    ColorSensor,
)
from pybricks.parameters import Port, Stop, Direction, Button, Color
from pybricks.tools import wait, StopWatch, DataLog
from pybricks.robotics import DriveBase
from pybricks.media.ev3dev import SoundFile, ImageFile

import utils
import defs

from models.motor_controller import MotorController, GrabableMotorController

ev3 = EV3Brick()
color_sensor = ColorSensor(Port.S1)
touch_sensor = TouchSensor(Port.S2)

motor_a = MotorController(Motor(Port.A), name="Motor A", scale=defs.MOTOR_A_SCALE)
motor_b = GrabableMotorController(
    Motor(Port.B),
    name="Motor B",
    scale=defs.MOTOR_B_SCALE,
    safeRange=defs.MOTOR_B_SAFE_RANGE,
)


def main():
    # Play startup sound
    ev3.speaker.play_file("../media/assets/dji_startup.adapted.wav")

    # Initialize motors
    motor_a.set_speed(defs.DEFAULT_MOTOR_MOVE_SPEED)
    motor_b.set_speed(defs.DEFAULT_MOTOR_MOVE_SPEED)

    # Measure motors
    motor_a.measure()
    motor_b.measure()

    ## 移动到夹取的位置
    touched_angle = 5  # 刚好触碰到按钮与能够夹取物件之间的夹角
    utils.moveToEdge(
        motor_a, touch_sensor, targetAngle=defs.MOTOR_A_SAFE_RANGE[0] + touched_angle
    )
    motor_a.rotate_to(defs.MOTOR_A_STANDBY_ANGLE, pauser=lambda: touch_sensor.pressed())

    ## ~~“夹取-移动到目标位置-释放-回到待机位置” 操作循环~~
    ## 移动到待机位置-夹取-移动到目标位置-释放 操作循环
    def operation(angle):
        motor_a.rotate_to(
            defs.MOTOR_A_STANDBY_ANGLE, pauser=lambda: touch_sensor.pressed()
        )
        motor_b.grab()
        motor_a.rotate_to(angle)
        motor_b.release()

    while True:
        color = color_sensor.color()
        if color == Color.RED:
            print("^ [INF] Color detected: RED")
            operation(defs.MOTOR_A_RED_ANGLE)
        elif color == Color.GREEN:
            print("^ [INF] Color detected: GREEN")
            operation(defs.MOTOR_A_GREEN_ANGLE)
        elif color == Color.BLUE:
            print("^ [INF] Color detected: BLUE")
            operation(defs.MOTOR_A_BLUE_ANGLE)
        elif color == Color.YELLOW:
            print("^ [INF] Color detected: YELLOW")
            operation(defs.MOTOR_A_YELLOW_ANGLE)
        else:
            print("^ [WAN] Color detected: UNKNOWN")
        wait(1000)


if __name__ == "__main__":
    main()
